#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"

class CameraPublisher : public rclcpp::Node
{
public:
    CameraPublisher() : Node("camera_publisher")
    {
        // 创建压缩图像发布者
        cam_pub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>("camera_2", 10);

        // 打开摄像头设备
        cap_.open("/dev/video0"); // 指定设备路径
        if (!cap_.isOpened())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open camera at /dev/video0!");
            return;
        }

        // 设置定时器，每30ms发布一次图像
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(30),
            std::bind(&CameraPublisher::timer_callback, this));
    }

private:
    cv::VideoCapture cap_;
    rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr cam_pub_;
    rclcpp::TimerBase::SharedPtr timer_;

    void timer_callback()
    {
        cv::Mat frame;
        if (cap_.read(frame))
        {
            publish_compressed_image(frame);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to read frame from camera!");
        }
    }

    void publish_compressed_image(cv::Mat image)
    {
        // 创建消息头并设置时间戳
        auto header = std_msgs::msg::Header();
        header.stamp = this->now();
        header.frame_id = "camera_frame";

        // 压缩图像
        std::vector<uchar> compressed_image_buffer;
        cv::imencode(".jpg", image, compressed_image_buffer, {cv::IMWRITE_JPEG_QUALITY, 50});

        // 创建并发布CompressedImage消息
        sensor_msgs::msg::CompressedImage compressed_msg;
        compressed_msg.header = header;
        compressed_msg.format = "jpeg";
        compressed_msg.data = std::vector<uint8_t>(compressed_image_buffer.begin(), compressed_image_buffer.end());

        cam_pub_->publish(compressed_msg);
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CameraPublisher>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}